#include <iostream>
#include <cstdlib>
#include <time.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <libplayerc++/playerc++.h>
#include "args.h"
#define RAYS 32
#include <time.h>
#include <signal.h>
#include <ctime>        
#include <sys/time.h>
#include "ekfblob.cpp"

using namespace std;
using namespace PlayerCc;
FILE *posoutput;
FILE *posoutput1;
FILE *laseroutput;
//FILE *lm;
FILE *mapin;
FILE *LOG;

//void getRobots(matlite &ROBOTS,int N_ROBOT);

//goto definitions
int N_VIA=5;
int N_giri = 1;
double viapointX[]={3,3,-3,-3,3};
double viapointY[]={1,-2,-2,2,2};
int vp=0;
int ig=1;

bool finish = false;
bool ODOM_ERROR=false;
int debug=0;

int main(int argc, char** argv){
// file di log e output

struct tm *current;
        time_t now;
        time(&now);
        current = localtime(&now);
        char outputdata[50];
        char logFile[50];       
        sprintf(outputdata,"output_data/loc-%d%d%d%d%d%d.txt",current->tm_year,current->tm_mon,current->tm_mday,current->tm_hour,current->tm_min,current->tm_sec);
sprintf(logFile,"log/LOG-%d%d%d%d%d%d.txt",current->tm_year,current->tm_mon,current->tm_mday,current->tm_hour,current->tm_min,current->tm_sec);


        LOG=fopen(logFile,"w");
        //posoutput=fopen(outputdata,"w");
        posoutput=fopen("output_data/loc_1robot.txt","w");        
	posoutput1=fopen("output_data/loc_2robot.txt","w");
        //lm=fopen("LM.txt","w");


//////////////////// mannaggia

//PlayerClient robot_0(gHostname, 6666);
PlayerClient robot_1(gHostname, 6667);
PlayerClient robot_2(gHostname, 6668);
PlayerClient robot_3(gHostname, 6669);
PlayerClient robot_4(gHostname, 6670);
//Position2dProxy pp_0(&robot_0, gIndex); //This is needed in every player client application
Position2dProxy pp_1(&robot_1, gIndex); //This is needed in every player client application
Position2dProxy pp_2(&robot_2, gIndex); //This is needed in every player client application
Position2dProxy pp_3(&robot_3, gIndex); //This is needed in every player client application
Position2dProxy pp_4(&robot_4, gIndex); //This is needed in every player client application


////////////////////////
        //ROBOT PRINCIPALE
        //These are initialised before the infinite loop
        PlayerClient robot(gHostname, gPort);   //This is needed in every player client application
        Position2dProxy pp(&robot, gIndex);     //This is needed in every player client application
        LaserProxy lp(&robot, gIndex);          //This is needed in every player client application
        BlobfinderProxy bp(&robot,0);
        ekfblob ekf;
	//ROBOT SECONDARIO
	PlayerClient robot1(gHostname, 6666);   //This is needed in every player client application
        Position2dProxy pp1(&robot1, gIndex);     //This is needed in every player client application
        LaserProxy lp1(&robot1, gIndex);          //This is needed in every player client application
        BlobfinderProxy bp1(&robot1,0);
        ekfblob ekf1;
	
	//ROBOT PRINCIPALE
        //EFK SLAM object
        double *laserdata;                      //Laser data array
        double x,y,yaw,oldx,oldy,oldyaw;        //Variables used to find change in displacement d_d and change in orientation d_t
       
 FILE *input;
char LOG_file[50];
sprintf(LOG_file,"log_1.txt");
        input=fopen("input.txt","r");  

        ekf.set(input,LOG_file);
 	fclose(input);


                              //EKF SLAM initialisation
        //oldx=ekf.ox;xest.val[1][0]
        //oldy=ekf.oy;
        //oldyaw=ekf.ot;
        oldx=ekf.xest.val[0][0];
        oldy=ekf.xest.val[1][0];
        oldyaw=ekf.xest.val[2][0];
	//ROBOT SECONDARIO
 	//EFK SLAM object
        double *laserdata1;                      //Laser data array
        double x1,y1,yaw1,oldx1,oldy1,oldyaw1;        //Variables used to find change in displacement d_d and change in orientation d_t
 
FILE *input1;
char LOG_file1[50];
sprintf(LOG_file1,"log_11.txt");

        input1=fopen("input1.txt","r");   
        ekf1.set(input1,LOG_file1);     
 fclose(input1);
  

                          //EKF SLAM initialisation
        //oldx=ekf.ox;xest.val[1][0]
        //oldy=ekf.oy;
        //oldyaw=ekf.ot;
        //oldx1=ekf1.xest.val[0][0];
        //oldy1=ekf1.xest.val[1][0];
        //oldyaw1=ekf1.xest.val[2][0];

 	oldx1=ekf1.xest.val[0][0];
        oldy1=ekf1.xest.val[1][0];
        oldyaw1=ekf1.xest.val[2][0];

        //file di log   
        fprintf(LOG,"  init x ,y, yaw :%4.3f/%4.3f/%3.2f\n",x,y,yaw*180.0/PI);
        printf("  init x ,y, yaw :%4.3f/%4.3f/%3.2f\n",x,y,yaw*180.0/PI);

        int i=0;
        while(!finish){
        //      while (debug!=5){
                                debug=debug+1;
                i=i+1;
                
                try{

                        //These are called during the inifinite recursive loop
                        robot.Read();                           //Obtain DR and laser data
                        x=pp.GetXPos();                         //Alternatively, you can use v*dt and w*dt if you know the sample time.
                        y=pp.GetYPos();
                        yaw=pp.GetYaw();

 			robot1.Read();                           //Obtain DR and laser data
                        x1=pp1.GetXPos();                         //Alternatively, you can use v*dt and w*dt if you know the sample time.
                        y1=pp1.GetYPos();
                        yaw1=pp1.GetYaw();

                        //printf("  %d iesimo x ,y, yaw :%4.3f/%4.3f/%3.2f\n",i,x,y,yaw*180.0/PI);
                        fprintf(LOG," %d iesimo x ,y, yaw :%4.3f/%4.3f/%3.2f\n",i,x,y,yaw*180.0/PI);

float n;                
if (ODOM_ERROR){
 n=rand() %100;
  n=n*0.00001;
}else {
 n =0;
}

                        double d_d=sqrt((oldx-x+n)*(oldx-x+n)+(oldy-y+n)*(oldy-y+n));
                        double d_t=yaw-oldyaw;
                        normalise(d_t);                         //Force all angles to be between +-180 degrees

 			double d_d1=sqrt((oldx1-x1+n)*(oldx1-x1+n)+(oldy1-y1+n)*(oldy1-y1+n));
                        double d_t1=yaw1-oldyaw1;
                        normalise(d_t1);     

                        //laserdata=new double[ekf.laserproxies];       //Set blank array equal to number of proxies in the laser
                        //for(int j=0;j<ekf.laserproxies;j++){
                        //      laserdata[j]=lp.GetRange(j);
                        //}

                int NBlob=bp.GetCount();        
        playerc_blobfinder_blob_t ListaBlob[NBlob];             
                                        printf(" ... ");
                for (int z=0; z<NBlob;z++){
                                playerc_blobfinder_blob_t blob=bp.GetBlob(z);
                                ListaBlob[z]=blob;
                }



                int NBlob1=bp1.GetCount();        
        playerc_blobfinder_blob_t ListaBlob1[NBlob1];             
                                        printf(" ... ");
                for (int z=0; z<NBlob1;z++){
                                playerc_blobfinder_blob_t blob1=bp1.GetBlob(z);
                                ListaBlob1[z]=blob1;
                }
        
                        double ltime=bp.GetDataTime();          //Get timestamp for laser
                        double ptime=pp.GetDataTime();          //Get timestamp for odometer
                        double lpdiff=abs(ltime-ptime);         //Find laser and odometer timestampt difference

			double ltime1=bp1.GetDataTime();          //Get timestamp for laser
                        double ptime1=pp1.GetDataTime();          //Get timestamp for odometer
                        double lpdiff1=abs(ltime1-ptime1);         //Find laser and odometer timestampt difference

                        ekf.predict(d_d,d_t);                   //EKF predict using d_d and d_t
			ekf1.predict(d_d1,d_t1);                   //EKF predict using d_d and d_t

                        //logdata
                        fprintf(posoutput,"%f %f %f %f %f %f %f %f\n",ekf.xest.val[0][0],ekf.xest.val[1][0],ekf.xest.val[2][0],x,y,yaw,d_d,d_t);
  			fprintf(posoutput1,"%f %f %f %f %f %f %f %f\n",ekf1.xest.val[0][0],ekf1.xest.val[1][0],ekf1.xest.val[2][0],x1,y1,yaw1,d_d1,d_t1);
                        fprintf(LOG,"stima dello stato - predict: %f %f %f %f %f %f %f %f\n",ekf.xest.val[0][0],ekf.xest.val[1][0],ekf.xest.val[2][0],x,y,yaw,d_d,d_t);
                        //printf("%d EKF:%4.3f/%4.3f/%3.2f DR:%4.3f/%4.3f/%3.2f LM: %d\n",i,ekf.xest.val[0][0],ekf.xest.val[1][0],ekf.xest.val[2][0]*180.0/PI,x,y,yaw*180.0/PI,(ekf.xest.nRow()-3)/2);

                        //Update map and log data
                        //for(int j=3;j<ekf.xest.nRow();j++){
                        //      fprintf(lm,"%f ",ekf.xest.val[j][0]);
                        //}

//getting info from other robots
        matlite ROBOTS;
        int N_ROBOT=6;
        ////////////////
        double xR,yR,tR;

// Robots 
//[0] x cord
//[1] y cord
//[2] t cord
//[3] port

ROBOTS.resize(4,N_ROBOT);


robot.Read();
robot1.Read();
robot_1.Read();
robot_2.Read();
robot_3.Read();
robot_4.Read();

//xR=pp1.GetXPos();                              //Alternatively, you can use v*dt and w*dt if you know the sample time.
//yR=pp1.GetYPos();
//tR=pp1.GetYaw();

ROBOTS.val[0][0]=ekf1.xest.val[0][0];
ROBOTS.val[1][0]=ekf1.xest.val[1][0];
ROBOTS.val[2][0]=ekf1.xest.val[2][0];
ROBOTS.val[3][0]=robot1.GetPort();

xR=pp_1.GetXPos();                              //Alternatively, you can use v*dt and w*dt if you know the sample time.
yR=pp_1.GetYPos();
tR=pp_1.GetYaw();

ROBOTS.val[0][1]=xR;
ROBOTS.val[1][1]=yR;
ROBOTS.val[2][1]=tR;
ROBOTS.val[3][1]=robot_1.GetPort();

xR=pp_2.GetXPos();                              //Alternatively, you can use v*dt and w*dt if you know the sample time.
yR=pp_2.GetYPos();
tR=pp_2.GetYaw();

ROBOTS.val[0][2]=xR;
ROBOTS.val[1][2]=yR;
ROBOTS.val[2][2]=tR;
ROBOTS.val[3][2]=robot_2.GetPort();

xR=pp_3.GetXPos();                              //Alternatively, you can use v*dt and w*dt if you know the sample time.
yR=pp_3.GetYPos();
tR=pp_3.GetYaw();

ROBOTS.val[0][3]=xR;
ROBOTS.val[1][3]=yR;
ROBOTS.val[2][3]=tR;
ROBOTS.val[3][3]=robot_3.GetPort();

xR=pp_4.GetXPos();                              //Alternatively, you can use v*dt and w*dt if you know the sample time.
yR=pp_4.GetYPos();
tR=pp_4.GetYaw();

ROBOTS.val[0][4]=xR;
ROBOTS.val[1][4]=yR;
ROBOTS.val[2][4]=tR;
ROBOTS.val[3][4]=robot_4.GetPort();

ROBOTS.val[0][5]=ekf.xest.val[0][0];
ROBOTS.val[1][5]=ekf.xest.val[1][0];
ROBOTS.val[2][5]=ekf.xest.val[2][0];
ROBOTS.val[3][5]=robot.GetPort();

//////////////////////
printf("Matrice ROBOTS:\n");
for (int z=0; z<N_ROBOT; z++){
printf("[%f %f %f %f] \n",ROBOTS.val[0][z],ROBOTS.val[1][z],ROBOTS.val[2][z],ROBOTS.val[3][z]);
}

                        ekf.update(ListaBlob,NBlob,d_t,lpdiff,ROBOTS);  //EKF update using laser data, d_t and lpdiff
 			ekf1.update(ListaBlob1,NBlob1,d_t1,lpdiff1,ROBOTS);  //EKF update using laser data, d_t and lpdiff
                        fprintf(LOG,"stima dello stato - update: %f %f %f %f %f %f %f %f\n",ekf.xest.val[0][0],ekf.xest.val[1][0],ekf.xest.val[2][0],x,y,yaw,d_d,d_t);

                        // goto commands . . . . .  . . . .. . . . .  . . . . 
                        if(N_giri!=0)
                                if (viapointX[vp] < x+0.2 && viapointX[vp] > x-0.2  && viapointY[vp] < y+0.2 && viapointY[vp] > y-0.2 ){
                                        vp=vp+1;
                                        robot.Read();
                                        pp.GoTo(viapointX[vp],viapointY[vp],0);
  					pp.GoTo(-3,-3,0);
					pp1.GoTo(0,2,0);
                                        std::cout << "goto starting, target: " << viapointX[vp]
                                                << ", " << viapointY[vp] << std::endl;
                                        std::cout << "indice target i: " << vp << std::endl;
                                        std::cout << "giro " << ig << std::endl;
                                }

                        if(vp>=N_VIA && ig<N_giri){
                                ig=ig+1;
                                vp=0;
                        }

                        if(vp>=N_VIA && ig>=N_giri){
                                finish= true;
                        }

                        oldx=x;
                        oldy=y;
                        oldyaw=yaw;    
			oldx1=x1;
                        oldy1=y1;
                        oldyaw1=yaw1;   
                }
                catch (PlayerCc::PlayerError & e)
                {
                        std::cerr << e << std::endl;
                        return -1;
                }

        }//THE END loop

        //fclose(lm);
        fclose(posoutput);
	fclose(posoutput1);
        fclose(LOG);

}

//void getRobots(matlite &ROBOTS,int N_ROBOT){


//}


